import numpy as np

def invert_transformation_matrix(matrix):
    # 提取旋转矩阵 R 和平移向量 t
    R = matrix[:3, :3]
    t = matrix[:3, 3]
    
    # 计算旋转矩阵的转置
    R_inv = R.T
    
    # 计算新的平移向量
    t_inv = -np.dot(R_inv, t)
    
    # 组装新的转换矩阵
    inverted_matrix = np.eye(4)
    inverted_matrix[:3, :3] = R_inv
    inverted_matrix[:3, 3] = t_inv
    
    return inverted_matrix

# 示例输入：4x4 的转换矩阵（IMU 到 cam0）
R_imu_to_cam0 = np.array([
    [0.9999394671768458, -0.0006750866130373227, -0.010982087240194097, -0.008085053740617219],
    [-0.0008786167836586301, -0.9998277580865089, -0.018538667488615344, -0.003058130608605723],
    [-0.010967680458228603, 0.01854719433690357, -0.9997678288320724, -0.033303585021934715],
    [0.0, 0.0, 0.0, 1.0]
])

# 计算 cam0 到 IMU 的转换矩阵
R_cam0_to_imu = invert_transformation_matrix(R_imu_to_cam0)

np.set_printoptions(precision=20, suppress=True)

print("IMU 到 cam0 的转换矩阵：")
print(R_imu_to_cam0)

print("\ncam0 到 IMU 的转换矩阵：")
print(R_cam0_to_imu)
